#include "cansocket.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"


#include<rclcpp_components/register_node_macro.hpp>
#include <string>
#include <memory>
#include <thread>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/array.hpp>
#include <chrono>
#include <memory>

#include "std_msgs/msg/byte_multi_array.hpp"
#include "speed_conclude.hpp"


using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class wheelPublisher : public rclcpp::Node 
{
public:
  wheelPublisher()
  : Node("wheel_node"),w_can("can0")
  {
    // subscription_ = this->create_subscription<std_msgs::msg::String>(          // CHANGE
    //   "topic", 10, std::bind(&wheelPublisher::topic_callback, this, std::placeholders::_1));
      //  subscription_ = this->create_subscription<std_msgs::msg::String>(
      //     "topic", 10, std::bind(&wheelPublisher::topic_callback, this, _1)); 
    publisher_ = this->create_publisher<std_msgs::msg::String>("wheel_info", 10);
    // timer_ = this->create_wall_timer(
    //   500ms, std::bind(&wheelPublisher::timer_callback, this));
    subscription_ = this->create_subscription<std_msgs::msg::ByteMultiArray>(
      "uart_send", 10, std::bind(&wheelPublisher::topic_callback, this, std::placeholders::_1));

      w_can.set_lun_speed(0,0);
      w_can.set_lun_speed(0,1);
      w_can.run();
     
  }

private:
  CanSocket w_can;
  speed_conclude sp_c;
   void topic_callback(const std_msgs::msg::ByteMultiArray::SharedPtr msse) 
    {
      unsigned char sd[8];
      int16_t speed,temp,ca_sp;
      float vt,vp,vcha;
      for(int i=0;i<8;i++)sd[i]=msse->data[i];
      temp=msse->data[3];
      speed=(temp-128)*5;
      // if(speed>0 && speed<100) speed=0;
      // if(speed<0 && speed>-100)speed=0;
       temp=msse->data[4];
       ca_sp=(temp-128)*1;
       if(ca_sp>0 && ca_sp<40) ca_sp=0;
       if(ca_sp<0 && ca_sp>-40)ca_sp=0;
      vp=ca_sp;
      vcha=vp*0.3*0.30/128.0;
      ca_sp=sp_c.conclude_rmp(vcha);
      vp=speed/5.0;
      vt=vp*0.5/128.0;  //最高速度 0.5米/秒
      speed = sp_c.conclude_rmp(vt);

      // sd[0]=speed>>8;
      // sd[1]=speed&0xff;
      w_can.set_lun_speed(speed+ca_sp,0);
      w_can.set_lun_speed(speed-ca_sp,1);
      // usleep(1000);
      // w_can.read_can();
      // w_can.write_can(sd,0x201);
      // usleep(1000);
      // w_can.read_can();
      // w_can.write_can(sd,0x202);
      RCLCPP_INFO(this->get_logger(), "I heard: '%x'  v=%d cha=%d", msse->data[3],speed,ca_sp);
    }
  
  void timer_callback() 

  {
    unsigned char sd[8];
    for(int i=0;i<8;i++)sd[i]=0x33;
    w_can.write_can(sd,0x333);
    // auto message = std_msgs::msg::String();
    // message.data = "Hello, world! " + std::to_string(count_++);
    // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    // publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  // rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;  
 //rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;  
  rclcpp::Subscription<std_msgs::msg::ByteMultiArray>::SharedPtr subscription_;  

  size_t count_;
};

int main(int argc, char * argv[])
{
  system("sudo ip link set can0 down");
  system("sudo ip link set can0 type can bitrate 250000");
  system("sudo ip link set can0 up");
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<wheelPublisher>());
  rclcpp::shutdown();
  return 0;
}